#include "PanShoot.h"

PanShoot* PanShoot::_instance = NULL;
 
PanShoot::PanShoot(NetworkTable* smartDashboard, DriverConsole* myDrive){
	//Create the turret motor PID. Lots of instantiation
	_rotatingTurretMotor = new Jaguar(ROTATING_TURRET_MOTOR_CHAN);
	_turretEncoder = new Encoder(TURRET_ENCODER_CHANNEL_A,TURRET_ENCODER_CHANNEL_B,true,Encoder::k1X);
	_turretEncoder->Start();
	_turretEncoder->Reset();
	_turretEncoder->SetPIDSourceParameter(Encoder::kDistance);
	
	_turretEncoder->SetDistancePerPulse(TURRET_ENC_DPP);
	_turretPID = new PIDController(TURRET_PID_P,TURRET_PID_I,TURRET_PID_D,_turretEncoder,_rotatingTurretMotor);
	_turretPID->Enable();
	_turretPID->SetOutputRange(-.2,.2);
	
	_shootTurretMotorTop = new MotorWrapper(SHOOT_TURRET_MOTOR_TOP_CHAN, MotorWrapper::speed, true);
	_shootTurretMotorTop->SetPID(SHOOTER_PID_P,SHOOTER_PID_I,SHOOTER_PID_D);
	_shootTurretMotorTop->SetEncoderCPP(SHOOTER_ENC_CPP);
	_shootTurretMotorBot = new MotorWrapper(SHOOT_TURRET_MOTOR_BOT_CHAN, MotorWrapper::speed, false);
	_shootTurretMotorBot->SetPID(SHOOTER_PID_P,SHOOTER_PID_I,SHOOTER_PID_D);
	_shootTurretMotorBot->SetEncoderCPP(SHOOTER_ENC_CPP);
	
	//Create the pitch motor PID. More instantiation
	_pitchTurretMotor = new MotorWrapper(PITCH_TURRET_MOTOR_CHAN, MotorWrapper::voltage, false);
	_angleSensor = new AnalogChannel(ANGLE_SENSOR_CHANNEL);
	
	_driveConsole = myDrive;
	_smartDashboard = smartDashboard;
	
	_isReady = false;
	_targetFound = false;
	_startSearch = true;
	_lastTargetAngle = 0;
	_lastRotBut = false;
	_angleNumClicks = 0;
	if (_angleSensor->GetVoltage() > 3)
		_canClick = false;
	else
		_canClick = true;
	
	_conveyor = ConveyorMaster::GetInstance(smartDashboard, myDrive);
}

PanShoot* PanShoot::GetInstance(NetworkTable* smartDashboard, DriverConsole* myDrive) {
	if (NULL == _instance) _instance = new PanShoot(smartDashboard, myDrive);
	return _instance;
}

PanShoot::~PanShoot() {
	delete _rotatingTurretMotor;
	delete _shootTurretMotorTop;
	delete _shootTurretMotorBot;
	delete _pitchTurretMotor;
}

bool PanShoot::CheckHAngle(){
	if (_turretPID->GetSetpoint() < 5)
		return true;
	return fabs((_turretPID->Get() - _turretPID->GetSetpoint())/_turretPID->GetSetpoint()) < ACCURACY; 
}
/*
float getVAngle() { 
	float raw = _stretchSensor->GetVoltage();
	raw -= PandroidSettings::STRETCH_MIN_VOLTAGE;
	raw *= PandroidSettings::STRETCH_VOLTAGE_TO_DEGREES;
	return raw; 
} */
bool PanShoot::CheckVAngle(){
	if (_pitchPID->GetSetpoint() < 5)
			return true;
	return fabs((_pitchPID->Get() - _pitchPID->GetSetpoint())/_pitchPID->GetSetpoint()) < ACCURACY; 
}

bool PanShoot::CheckSpeed(){
	bool topClose = fabs((_shootTurretMotorTop->GetState() - _shootTurretMotorTop->GetSetpoint())/_shootTurretMotorTop->GetSetpoint()) < ACCURACY; 
	bool botClose = fabs((_shootTurretMotorBot->GetState() - _shootTurretMotorBot->GetSetpoint())/_shootTurretMotorBot->GetSetpoint()) < ACCURACY;
	return topClose && botClose;
}

void PanShoot::SetSpeed(float speed){
	_shootTurretMotorTop->SetSetpoint(speed); 
	_shootTurretMotorBot->SetSetpoint(speed + SPIN_VEL);
}

void PanShoot::SetVAngle(float angle){
	_pitchTurretMotor->SetSetpoint(min(max(TURRET_MIN_VANGLE,angle),TURRET_MAX_VANGLE));
}

float PanShoot::GetStretchVoltage(float angle) {
	return angle;
}

void PanShoot::SetHAngle(float angle){
	_turretPID->SetSetpoint(min(max(TURRET_MIN_HANGLE,angle),TURRET_MAX_HANGLE));
	printf("Setpoint: %f Error: %f\n", 180*_driveConsole->GetJoystickX(DriverConsole::JoystickLeft), _turretPID->GetError());
	//printf("INput angle: %f Output angle: %f", angle,min(max(TURRET_MIN_HANGLE,angle),TURRET_MAX_HANGLE));
}

void PanShoot::Rotate180() {
	SetHAngle(_rotatingTurretMotor->Get() + 180);
}

void PanShoot::Teleop() {
	if (!_driveConsole->GetButtonState(BTN_MANUAL_OVERRIDE)) {
		_targetFound = _smartDashboard->GetBoolean("targetFound");
		
		if (_targetFound && CheckSpeed() && CheckVAngle() && CheckHAngle()) 
			_isReady = true;
		else
			_isReady = false;
		
		if (_targetFound) {
			_startSearch = true;
			_conveyor->Teleop(_isReady);
			_lastTargetAngle = _smartDashboard->GetDouble("goalHAngle");
			SetSpeed(_smartDashboard->GetDouble("goalSpeed"));
			SetHAngle(_lastTargetAngle);
			SetVAngle(_smartDashboard->GetDouble("goalVAngle"));
			
		} else {
			Search();
		}
	} else {
		if (_driveConsole->GetButtonState(BTN_ROT_180)) {
			if (!_lastRotBut)
				Rotate180();
		} else
			_lastRotBut = false;
		
		SetSpeed(_driveConsole->GetPotential(POT_TURRET_SPEED));
		SetHAngle(_driveConsole->GetPotential(POT_TURRET_HANGLE));
		SetVAngle(_driveConsole->GetPotential(POT_TURRET_VANGLE));
	}
}

void PanShoot::GoToAngle(float angle) {
	angle = min(max(angle,MIN_PITCH_ANGLE),MAX_PITCH_ANGLE);
	double length = sqrt(1045.1 - 999*cos((angle-18.2)*3.14/180));
	printf("Length: %.2f\n", length);
	int clicks = (int) ((16.5 - length)*3.28);
	printf("Clicks: %d\n", clicks);
	if (_angleNumClicks + 1 < clicks) {
		_pitchTurretMotor->SetSetpoint(1);
		if (_angleSensor->GetVoltage() > 4.5 && _canClick) {
			_angleNumClicks++;
			_canClick = false;
			printf("Num Clicks: %d\n", _angleNumClicks);
		}
	} else if (_angleNumClicks - 1 > clicks) {
		if (_angleSensor->GetVoltage() > 4.5 && _canClick) {
			_angleNumClicks--;
			_canClick = false;
			printf("Num Clicks: %d\n", _angleNumClicks);
		}
		_pitchTurretMotor->SetSetpoint(-1);
	} else
		_pitchTurretMotor->SetSetpoint(0);
	

	
	if (_angleSensor->GetVoltage() < 4) {
		_canClick = true;
		printf("Reset\n");
	}
}

void PanShoot::MovePitch(float input) {
	if (input > .5)
		_pitchTurretMotor->SetSetpoint(1);
	else if (input < -.5)
		_pitchTurretMotor->SetSetpoint(-1);
	else
		_pitchTurretMotor->SetSetpoint(0);
}

bool PanShoot::ZeroPitch() {
	if (_angleNumClicks > 1) {
		_pitchTurretMotor->SetSetpoint(-1);
		if (_angleSensor->GetVoltage() > 4.5 && _canClick) {
			_angleNumClicks--;
			_canClick = false;
			printf("Num Clicks: %d\n", _angleNumClicks);
		}
	}  else {
		_pitchTurretMotor->SetSetpoint(0);
		return true;
	}
	if (_angleSensor->GetVoltage() < 4) {
		_canClick = true;
	}
	return false;
}

void PanShoot::Search() {
	if (_startSearch)
		if (_rotatingTurretMotor->Get() > _lastTargetAngle)
			_rotatingTurretMotor->Set(TURRET_MIN_HANGLE+5);
		else
			_rotatingTurretMotor->Set(TURRET_MAX_HANGLE-5);
	if ((_rotatingTurretMotor->Get()) > TURRET_MAX_HANGLE-4) {
		_startSearch = false;
		_rotatingTurretMotor->Set(TURRET_MIN_HANGLE+5);
	} else if (_rotatingTurretMotor->Get() < TURRET_MIN_HANGLE+4){
		_startSearch = false;
		_rotatingTurretMotor->Set(TURRET_MAX_HANGLE+5);
	}
}

void PanShoot::Update() {
	_smartDashboard->PutDouble("currentSpeed", _shootTurretMotorTop->GetState());
	_smartDashboard->PutDouble("currentHAngle", _rotatingTurretMotor->Get());
	_smartDashboard->PutDouble("currentVAngle", _pitchTurretMotor->GetState());
}

void PanShoot::Autonomous() {
	
}

void PanShoot::PrintDump() {
	//_shootTurretMotorTop->SetSetpoint(1000*_driveConsole->GetJoystickY(DriverConsole::JoystickLeft));
	//_shootTurretMotorBot->SetSetpoint(1000*_driveConsole->GetJoystickY(DriverConsole::JoystickLeft));
	static bool zeroing = false;
	if (_driveConsole->GetJoystickButton(DriverConsole::JoystickLeft,1) || zeroing) {
		if (ZeroPitch())
			zeroing = false;
		else 
			zeroing = true;
	} else	 
	//GoToAngle(_driveConsole->GetJoystickY(DriverConsole::JoystickRight)*22.5+45);
	MovePitch(_driveConsole->GetJoystickY(DriverConsole::JoystickRight));
	SetHAngle(_driveConsole->GetJoystickX(DriverConsole::JoystickLeft)*180);
	static float p = 0;
	static float i = 0;
	static float d = 0;
	static bool lastP = false;
	static bool lastI = false;
	static bool lastD = false;
	_turretPID->SetPID(p,i,d);
	_turretPID->Enable();
	
	if (_driveConsole->GetJoystickButton(DriverConsole::JoystickLeft,6) && !lastP) {
		lastP = true;
		p += .001;
	} else if (!_driveConsole->GetJoystickButton(DriverConsole::JoystickLeft,6))
		lastP = false;
	if (_driveConsole->GetJoystickButton(DriverConsole::JoystickLeft,7) && !lastI) {
		lastI = true;
		i += .001;
	} else if (!_driveConsole->GetJoystickButton(DriverConsole::JoystickLeft,7))
		lastI = false;
	if (_driveConsole->GetJoystickButton(DriverConsole::JoystickLeft,8) && !lastD) {
		lastD = true;
		d -= .001;
	} else if (!_driveConsole->GetJoystickButton(DriverConsole::JoystickLeft,8))
		lastD = false;
	if (_driveConsole->GetJoystickButton(DriverConsole::JoystickLeft,9)) {
		p = 0;
		i = 0;
		d = 0;
	}
	printf("P: %f I: %f D: %f\n", p, i, d);
	printf("Encoder: %f", _turretEncoder->GetDistance());
	
	//printf("Top Encoder: %f Top Voltage: %f\n", _shootTurretMotorTop->GetState(), _shootTurretMotorTop->GetVoltage());
	//printf("Botton Encoder: %f Bottom Voltage: %f\n", _shootTurretMotorBot->GetState(), _shootTurretMotorBot->GetVoltage());
	//_shootTurretMotorTop->SetPID(p,i,d);
	//_shootTurretMotorBot->SetPID(p,i,d);
	//_shootTurretMotorTop->SetSetpoint(3000*_driveConsole->GetJoystickX(DriverConsole::JoystickLeft));
	//_shootTurretMotorBot->SetSetpoint(3000*_driveConsole->GetJoystickX(DriverConsole::JoystickLeft));
	//printf("Turret Encoder: %f\n",_turretEncoder->GetDistance());
	//printf("Turret Voltage: %.2f\n",_turretPID->Get());
	//printf("Turret Setpoint: %.2f\n",_turretPID->GetSetpoint());
	
	//printf("Top Shooter Motor Enc: %.2f\n", _shootTurretMotorTop->GetState());
	//printf("Bot Shooter Motor Enc: %.2f\n", _shootTurretMotorBot->GetState());
	//printf("Pitch Motor Temp: %.2f\n", _pitchTurretMotor->GetTemp());
}
